perm filename PINTRP.PAL[PNT,HE]9 blob
sn#506107 filedate 1980-03-27 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00021 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00003 00002 data transfer macros: SNDINT,SNDFP,FTAPE
C00005 00003
C00008 00004 pushinti,pushsci
C00010 00005 data transfer : PUSHINTI,ARTVAL,RTVAL,AGTVAL,RTARR,FRVAL
C00021 00006 RTLEVS - returns leveloffset info of stack in integer buffer
C00023 00007 PAFFIX,PUNFIX
C00028 00008 display: DISVT05
C00029 00009 PSPROUT: used with COBEGIN
C00031 00010 RCASE: used with CASE
C00033 00011 relative jumps: RFRCHK,RJMP,RJMPC,RFOREND
C00039 00012 printing routines: PRVAL,PRINTI,PRINTC
C00044 00013 supplementary motions: pmove,tadrive,tddrive,gather,rforce,setstf,setspeed
C00054 00014 supplementary functions: uparrow,dwnarrow,alpha,dollar,swap,vneg,vsmul,ftof
C00058 00015 functions: sqrt,sin,cos,asin,acos,tan,atan2,log,exp
C00059 00016 armreach- can arm reach here?
C00061 00017 procedure handling: GTBLK
C00063 00018 more stack ops: gtint,gvals,chngs
C00065 00019 components of data types: CHCMP,GTCMP
C00068 00020 signal,wait,cmpwait,cmvar,cmfil,pkvar
C00070 00021 return from POINTY : pdone
C00071 ENDMK
C⊗;
COMMENT ⊗ data transfer macros: SNDINT,SNDFP,FTAPE
⊗
.MACRO SNDINT X
MOV X,@INTPTR
ADD #2,INTPTR
.ENDM
.MACRO SNDFP X
STF X,@FPPTR
ADD #4,FPPTR
.ENDM
.MACRO SNDFIN X
STCFI X,@INTPTR
ADD #2,INTPTR
.ENDM
.MACRO FETCHF A
LDF @IPC(R4),A ;get the floating point arg
ADD #4,IPC(R4) ;Bump IPC twice
.ENDM
;; routine for transferring a block of fp data from 11 to 10
;; R0 has address of data, R1 has # FP numbers to transfer
;; R0,R1,AC0 are garbaged
COMMENT ⊗
FTAPE: TST R1
BEQ 2$
PUSH <R2>
MOV FPPTR,R2
1$: LDF (R0)+,AC0
STF AC0,(R2)+
SOB R1,1$
MOV R2,FPPTR
POP <R2>
2$: RTS PC
⊗ ;
MKVT: ;Following three numbers are components of vector
FETCHF AC1 ;Fetch arg1 (X)
FETCHF AC2 ;Fetch arg2 (Y)
FETCHF AC3 ;Fetch arg3 (Z)
JMP VMAKE0 ; return from VMAKE0
;following 3 numbers are euler angle values
MKRT: MOV #PZHAT,-(R3) ;put axis of rotation
JSR PC,PUSHSCI ;get the amount to rotate by
JSR PC,VSAXWR ; make the rot
MOV #PYHAT,-(R3)
JSR PC,PUSHSCI
JSR PC,VSAXWR
JSR PC,TTMUL
MOV #PZHAT,-(R3)
JSR PC,PUSHSCI
JSR PC,VSAXWR
JSR PC,TTMUL
RTS PC
; following 6 numbers are euler angle values
MKTR: JSR PC,MKVT
JSR PC,MKRT
JSR PC,SWAP
JSR PC,TMAKE
CCC
RTS PC
ARRLD: JSR PC,ARRSIZ ; get the array size and LOC[env entry first]
; R0←size, R1←LOC;
PUSH <R2>
MOV R1,-(SP) ; (SP)←LOC[first env entry]
MOV R0,R2
FETCH R0 ; get type of array
ASL R0 ; compute index into appropriate routine table
MOV 1$-2(R0),2$ ; put appropriate name into 2$
MOV (SP),R0 ; initialize properly
4$: PUSH <R2>
JSR PC,@2$ ; execute appropriate routineto get value into stack
MOV 2(SP),R0
ADD #4,2(SP)
JSR PC,CHNG1
POP <R2>
SOB R2,4$
6$: TST (SP)+
POP <R2>
CCC
RTS PC
DATA
1$:: .WORD PUSHSCI
.WORD MKVT
.WORD MKRT
.WORD MKTR
.WORD MKTR
.WORD NOOP
.WORD NOOP
2$:: .WORD 0
CODE
; pushinti,pushsci
COMMENT ⊗
; copy nth element on the stack to the top
COPY: FETCH R0 ;Pick up argument.
COPY0: ADD R0,R0 ;Double R0 to make it in bytes
ADD R3,R0 ;R0 ← LOC[stack element to be copied to top]
MOV (R0),-(R3) ;Copy it onto top of stack.
CCC ;Clear condition code.
RTS PC ;Done
REPLAC: FETCH R0 ;Pick up argument.
ADD R0,R0 ;Double R0 to make it in bytes
ADD R3,R0 ;R0 ← LOC[stack element to be copied into]
MOV (R3)+,(R0) ;Copy verge of stack into it.
CCC ;Clear condition code.
RTS PC ;Done
POPV: TST (R3)+ ;Pop stack
CCC ;Clear condition code.
RTS PC ;Done
⊗;
PUSHSCI:
; The argument is a (2 word) floating point number. Make a scalar out of it and
; push that scalar onto stack.
LDF @IPC(R4),AC0;get the floating point arg
ADD #4,IPC(R4) ;Bump IPC twice
BR PUSHREAL ;execute common code
PUSHINTI:
; The argument is an integer. Make a scalar out of it and
; push that scalar onto stack.
FETCH R0
PUSHI0: LDCIF R0,AC0 ;convert to real
PUSHREAL:
JSR PC,NOCMP
JSR PC,GETSCA ;R0 ← -(R3) ← LOC[new scalar block]
STF AC0,@(R3) ;Store result
JSR PC,YESCMP
CCC ;Clear condition code.
RTS PC ;Done
; data transfer : PUSHINTI,ARTVAL,RTVAL,AGTVAL,RTARR,FRVAL
COMMENT ⊗
routines to facilitate data transfer to POINTY interface
XX is scalar index; Y is leveloffset of array element
AGTVAL XX,Y = PUSHINTI XX; GTVAL Y
ACHNGE XX,Y = PUSHINTI XX; CHNGE Y
ARTVAL XX,Y = AGTVAL XX,Y; RTVAL
RTARR Y returns #elements and value of array offset Y
RTVAL is used to transfer the top element of stack to the return buffer
⊗;
AGTVAL: JSR PC,PUSHINTI ; get value of index to array
JMP GTVAL ; now get the offset of the array
CCHNGE: MOV (R3),-(R3) ; copy value of top element in stack
JMP CHNGE ; now do the assignment
CACHNG: MOV (R3),-(R3) ; copy value of top element in stack
ACHNGE: JSR PC,PUSHINTI ; get value of index to array
JMP CHNGE ; now update value of the array
CRTVAL: MOV (R3),R0 ; return top of stack without popping
JMP RTVAL0
FRVAL: FETCH <R0> ; get offset
FRVAL0: JSR PC,GETARG ; R0←LOC[environment entry]
BIT #HDRTYP,(R0) ; check header exists
BNE 1$
JSR PC,MFRAME ; make frame header
1$: MOV 2(R0),R0 ; R0←LOC[frame header]
PUSH <R0> ; save R0
ADD #CALCS,R0 ; R0←LOC[beginning of calculator list]
2$: MOV (R0),R0 ; R0←LOC[next calcualtor to check]
BEQ 6$ ; Make sure there is something there
BIT #AFXTYP,TYPE(R0); Make sure it is an affixment
BEQ 2$
BIT #FRAME2,TYPE(R0); Check if second frame in affixment
BNE 2$ ; If not, go check the next calculator
3$: BIT #EXPTRN,TYPE(R0); Is it an explicit trans?
BEQ 4$
MOV @TRANS(R0),R0 ; R0←LOC[trans]
BR 5$
4$: MOV TRANS(R0),R0 ; implicit trans
5$: POP <R1> ; get SP to correct state
JMP PC,RTVAL0 ; retrun from RTVAL0
6$: POP <R0>
JSR PC,NOCMP
CALL GETVAL,<R0> ; R0←Value
JSR PC,YESCMP
JMP PC,RTVAL0 ; return from RTVAL0
RTARR: JSR PC,ARRSIZ ; get array size
; R0←array size, R1←LOC[first env entry]
SNDINT R0
PUSH <R2>
PUSH <R1> ; (SP)←LOC[env entry]
MOV R0,R2 ; R2←#elements
2$: MOV (SP),R0 ; R0←LOC[env entry]
ADD #4,(SP) ; (SP)←next environment entry
JSR PC,GVAL1 ; (R3)←LOC[value cell]
JSR PC,RTVAL ; return the element value
SOB R2,2$
TST (SP)+ ; dont need the value of last push
POP <R2> ; get back the initial value of R2
CCC
RTS PC ; and return
; following routine returns parameter values to the 10 and returns
; the following register values:
; R0←#elements in the array
; R1←LOC[env entry for first element]
RTPARS: FETCH R0 ; get offset of the array we are interested in
SNDINT #XRTPARS ; send back info to 10
SNDINT R0 ; send back arrayoffset number to 10
PUSH <R2> ; save R2
PUSH <INTPTR> ; save location of INTPTR for later use
ADD #2,INTPTR ; increment the value of intptr
JSR PC,GETENV ; get environment pointer in R0
MOV 2(R0),R2 ; R2←LOC[array header]
MOV (R2)+,R0 ; R0←# of dimensions of array
SNDINT R0 ; return # of dimensions
MOV #1,-(SP) ; compute number of elements in array
1$: MOV (R2)+,R1 ; R1←(ub[i]- lb[i])*mult[i]
SNDINT R1 ; return upper bound
SNDINT (R2) ; return lower bound
SUB (R2)+,R1 ;
SNDINT (R2)+ ; return multiplier
INC R1 ; add 1
MUL (SP),R1 ; (upper-lower+1)*amount so far
MOV R1,(SP) ;
SOB R0,1$ ; repeat for all the dimensions
MOV (SP)+,R1 ; R1←# of elements in array
POP <R0>
MOV R1,(R0) ; and send it to the buffer
MOV R1,R0 ; R0←#of elements
MOV R2,R1 ; R1←LOC[env entry of first element]
POP <R2> ; get back the initial value of R2
CCC
RTS PC ; and return
ARRSIZ: FETCH R0 ; takes array offset in R0 and returns
; R0←#elements in array
; R1←LOC[env entry of first element]
ARRSZ0::PUSH <R2>
JSR PC,GETENV ; get environment pointer in R0
MOV 2(R0),R2 ; R2←LOC[array header]
MOV (R2)+,R0 ; R0←#dimensions of array
MOV #1,-(SP) ; compute # of elements in array
1$: MOV (R2)+,R1 ; R1←(UB[i]-LB[i]+1)
SUB (R2)+,R1
INC R1
TST (R2)+
MUL (SP),R1
MOV R1,(SP)
SOB R0,1$
MOV (SP)+,R0
MOV R2,R1
POP <R2>
CCC
RTS PC
ARRINI: JSR PC,RTPARS ; get the array size and LOC[env entry first]
PUSH <R2>
MOV R1,-(SP) ; (SP)←LOC[first env entry]
MOV R0,R2
MOV (SP),R0
CMP #SCLTYP,(R0) ; scalar array
BNE 2$
MOV #SC0,1$
BR 4$
2$: CMP #VECTYP,(R0) ;vector array
BNE 3$
MOV #VT0,1$
BR 4$
3$: CMP #TRNTYP,(R0) ;trans array
BNE 5$
MOV #TR0,1$ ; niltrans
BR 4$
5$: CMP #EVNTYP,(R0) ; check for events
BEQ 6$
ALERR 7$
4$: MOV 1$,-(R3) ; push appropriate zero value into the stack
MOV (SP),R0
ADD #4,(SP)
JSR PC,CHNG1
SOB R2,4$
6$: TST (SP)+
POP <R2>
CCC
RTS PC
DATA
1$: 0
7$: ASCIE /TRYING TO INITIALIZE ARRAY OF UNEXPECTED DATA TYPE/
CODE
ARTVAL: JSR PC,AGTVAL ; get the value of the array element
RTVAL: ; now output the value
MOV (R3)+,R0 ; pop the top element R0←loc[value cell]
RTVAL0: MOV #1,R1 ; counter for counting number of elements
CMPB #TRNID,TAGID(R0) ;A trans?
BEQ 1$
CMPB #VCTID,TAGID(R0) ;A vector?
BEQ 2$
BR 3$ ;Must be a scalar
1$: JSR PC,EULER
MOV #EDAT,R0
MOV #4,R1
2$: ADD #2,R1
3$: LDF (R0)+,AC0 ;load element into AC0
STF AC0,@FPPTR ;move it into return buffer
ADD #4,FPPTR ;update the pointer in the return buffer
SOB R1,3$ ;get the next element
RTS PC
EULER: MOV #EDAT,R1
JSR PC,@LEULER ; now recorrect
MOV #EDAT+14,R1 ; value of THETA
LDF (R1),AC0 ; get value of O computed by euler in armcode
SUBF F90,AC0
STF AC0,(R1)+
LDF (R1),AC0 ; PHI=A+90
ADDF F90,AC0
STF AC0,(R1)
RTS PC
DATA
F90: .FLT2 90.0
F180: .FLT2 180.0
EDAT: .BLKW 30
.WORD 1 ; scalar 0
SC0: .FLT2 0.0
.WORD 2 ; vector 0
VT0:: .FLT2 0.0,0.0,0.0,1.0
.WORD 2 ; yhat
PYHAT: .FLT2 0.0,1.0,0.0,1.0
.WORD 2 ; zhat
PZHAT: .FLT2 0.0,0.0,1.0,1.0
.WORD 3 ; niltrans
TR0: .FLT2 1.0,0.0,0.0
.FLT2 0.0,1.0,0.0
.FLT2 0.0,0.0,1.0
.FLT2 0.0,0.0,0.0
CODE
; RTLEVS - returns leveloffset info of stack in integer buffer
RTLEVS:
COMMENT ⊗ Returns offset of top element in the stack if simple variable: if it is
an array, returns the offset and the index sequentially. This does not
affect the stack. R0 and R1 are garbaged.
⊗
MOV R3,R1 ;Use temporary stackpointer
LDF @(R1)+,AC0 ;Get value of top element of stack
STCFI AC0,R0 ;convert into integer and put in R0
MOV R0,@INTPTR ;and store into integer buffer
ADD #2,INTPTR ;and increment integer buffer pointer
PUSH <R1> ;Since GETENV will clobber it
JSR PC,GETENV ;Get the environment pointer in R0
POP <R1> ;TO recover R1
BIT #ARYTYP,(R0) ;Do we have an array to access?
BEQ 10$
PUSH <R2>
MOV 2(R0),R2 ;R2 ← LOC[array header]
MOV (R2)+,R0 ;R0 ← # of dimensions of array
POP <R2>
3$: LDF @(R1)+,AC0 ;Get value of subscript
STCFI AC0,@INTPTR ;Ship it into integer buffer
ADD #2,INTPTR ;update the pointer
SOB R0,3$ ;Do all the subscripts
10$: RTS PC ;Return with R0 and R1 garbaged
; PAFFIX,PUNFIX
PAFFIX:
COMMENT ⊗ AFFIX together the two currently top elements
and return their offsets in the integer buffer.
⊗
SNDINT #XAFFIX ;return affix code
JSR PC,RTLEVS ;return the offset to the 10
JSR PC,GTINT ;Get first frame offset
JSR PC,GETARG ;R0 ← LOC[environment entry]
BIT #HDRTYP,(R0) ;Test access type
BNE 1$
JSR PC,MFRAME ;If necessary make a new frame header
1$: MOV 2(R0),R2 ;R2 ← LOC[first frame header]
JSR PC,RTLEVS ;return the offset to he 10
JSR PC,GTINT ;Get second frame offset
JSR PC,GETARG ;R0 ← LOC[environment entry]
BIT #HDRTYP,(R0) ;Test access type
BNE 2$
JSR PC,MFRAME ;If necessary make a new frame header
2$: MOV 2(R0),R1 ;R1 ← LOC[second frame header]
MOV @(R4),@INTPTR ;Get affixment code and return it
ADD #2,INTPTR ;increment the integer pointer
JMP AFFIX0 ;jump into main affix routine and return from there
PUNFIX:
COMMENT ⊗ return the offsets of the two top elements on the
stack and unfix them
⊗
MOV #2,4$
SNDINT #XUNFIX ;return unfix code
JSR PC,RTLEVS ;return offset to the 10
JSR PC,GTINT ;Get first frame offset
JSR PC,GETARG ;R0 ← LOC[environment entry]
BIT #HDRTYP,(R0) ;Check header exists
BEQ 1$ ; if not quit
MOV 2(R0),R2 ;R2 ← LOC[first frame header]
DEC 4$
1$: JSR PC,RTLEVS ;return offset of the second frame
JSR PC,GTINT ;Get second frame offset
JSR PC,GETARG ;R0 ← LOC[environment entry]
BIT #HDRTYP,(R0) ;Check header exists
BEQ 3$ ; if not quit
MOV 2(R0),R1 ;R1 ← LOC[second frame header]
DEC 4$
2$: BNE 3$
JMP UNFIX0 ; jump into main interpreter routine returning from there
3$: RTS PC ; return from here
DATA
4$: 0
CODE
; display: DISVT05
DISVT05:
FETCH <R0>
TST R0 ;R0=0 → display - R0=1 → nodisplay
BNE 1$ ;go to stop display
MOVB #COFF+30,CURYXAL ;trick display routine to think we are at bottom
MOV #1,FRMDDT ;forces display to update titles
1$: MOV R0,DSPOK
RTS PC
; PSPROUT: used with COBEGIN
PSPROUT:
FETCH <R2> ;R2←# of statements
MOV R2,R0
ASH #1,R0
INC R0
JSR PC,GTFREE
MOV R2,R1 ; R1← # of interpreters to spawn
PUSH <R0> ; save offset of new buffer (1)
PUSH <IPC(R4)> ;save current value of ipc (2)
1$: FETCH <R2> ;get the offset from beginning of sprout
ASH #1,R2 ;get byte offset
ADD (SP),R2 ;add the absolute address
MOV R2,(R0)+ ;stick it into new buffer
FETCH <(R0)+> ;increment the zero - better be zero
SOB R1,1$
FETCH <(R0)+> ; increment one more term, better be zero
TST (SP)+ ; pop value of old ipc (1)
MOV IPC(R4),R1 ; save current IPC value
MOV (SP),IPC(R4); change ipc value to beginning of buffer
PUSH <R1> ; and put old ipc value into the stack (2)
JSR PC,SPROUT ;jump into main AL routine
POP <IPC(R4)> ;restore the ipc value (1)
POP <R0> ;R0←address of buffer (0)
JSR PC,RLFREE ;release the buffer
CCC ;Clear condition code.
RTS PC ;Done
; RCASE: used with CASE
COMMENT ⊗ this routine assumes that the code following is similar to that
following the AL case statement, including range numbers. However, labels
are assumed to be relative to the first label, so that this routine sets
up a new temporary block with the absolute addresses and
then calls AL CASE statement before returning to release the block
⊗;
RCASE: FETCH <R2> ; R2←range
MOV R2,R0
BPL 1$ ; get the absolute value
NEG R0
1$: ADD #2,R0 ; # of labels = R0 + 1, so add 1 for the extra label and
; 1 for the value of R2
PUSH <R0> ; (1)
JSR PC,GTFREE ; get a block of free storage
POP <R1> ; (2)
DEC R1 ; R1← range +1 ,i.e. # of labels
PUSH <R0> ; save address of free storage block(1)
PUSH <IPC(R4)> ; save current IPC(2)
MOV R2,(R0)+ ; 1st word in block=signed range
2$: FETCH <R2>
ASL R2 ; change relative position into bytes
ADD (SP),R2 ; ipc address
MOV R2,(R0)+ ; and push into the block
SOB R1,2$ ; do for all labels
TST (SP)+ ; pop top element, dont need address anymore(1)
MOV (SP),IPC(R4); put address of this new auxilliary block of labels into ipc
JSR PC,CASE ; and jump into AL's case statement
POP <R0> ; now go release the space(0)
JSR PC,RLFREE
CCC
RTS PC
; relative jumps: RFRCHK,RJMP,RJMPC,RFOREND
COMMENT ⊗ These routines are parallel to the jump and transfer of control
routines in AL. The relative jumps are needed to produce
position independent pcode for the bodies of procedures
⊗
RJMP:
;Takes one argument: the relative offset of new address.
MOV @IPC(R4),R0 ; get the offset
ASL R0 ; change to bytes
ADD R0,IPC(R4) ; increment IPC by the offset
CCC ;Clear condition code.
RTS PC ;Done
RJMPC: ;Parallel to JUMPC in INTERP.PAL[AL,HE]
LDF @(R3)+,AC0 ;Get value of boolean
CFCC ;copy condition codes
BEQ 1$ ;if false succeed - take branch
BMPIPC ;skip over address
RTS PC ; & return
1$: MOV @IPC(R4),R0 ; get the offset
ASL R0 ; change to bytes
ADD R0,IPC(R4) ; branch
RTS PC ; & return
RFRCHK: ; change parallel routine in PINTRP.PAL when you change this
;Assume that the stack has, from surface in, the increment, the
; final value, and the control variable's value, all of which are
; scalar values. If (FINAL-CONVAR)*(INCREMENT) ≥ 0 then this is a
; no-op; otherwise, jump to the destination (end of FOR body) & clean up stack
;Arguments: destination.
JSR PC,GTARGS ;R0 ← LOC[variable environment entry] replaces 1st 2 lines of FORCHK
MOV 4(R3),2(R0) ;Store pointer to current value
LDF @2(R3),AC0 ;AC0 ← final value
SUBF @4(R3),AC0 ;AC0 ← final - current
MULF @(R3),AC0 ;AC0 ← (final - current)*increment
FETCH R0 ;R0 ← offset to destination
ASL R0 ;change to bytes
CFCC
BGE 1$ ;Shall this be a no-op?
BACKIPC ;since pointing at wrong place
ADD R0,IPC(R4) ;update the new IPC
ADD #6,R3 ;Pop the inc, final & control var off of the stack
1$: CLR R0
RTS PC ;Done
RFOREND: ;Interpreter routine
;Assume that the stack has, from surface in, the increment, the
; final value, and the control variable's value, all of which are
; scalar values. Copy the step size and the current value, add them
; and replace the current value. Then jump to the start of the loop.
JSR PC,NOCMP ;Don't compact for a bit
MOV (R3),-(R3) ;Copy step size
MOV 6(R3),-(R3) ;Copy current value
JSR PC,SADD ;Add them
MOV (R3)+,4(R3) ;Update the current value
JSR PC,YESCMP ;Okay to compact again
BR RJMP ;Now jump to start of for loop(note relative jump)
; printing routines: PRVAL,PRINTI,PRINTC
PRINTC: MOV IPC(R4),R0 ; prints single character
BMPIPC
JMP PRINT0
PRINTI: FETCH <-(SP)> ; string printing this will replace RPRINT
; (SP)←# of words to be printed
ASL (SP) ; convert to bytes
MOV IPC(R4),R0 ; R0←starting address of string
ADD (SP)+,IPC(R4) ; update the IPC
JMP PRINT0
COMMENT ⊗
RPRINT: MOV @IPC(R4),R0
ASL R0
ADD IPC(R4),R0 ; put absolute address into R0 of string
BMPIPC
JMP PRINT0
⊗;
TACK:
COMMENT ⊗ R1 = LOC[ascie string to tack on], R0 = LOC[where to put
it]. Returns R0 ← next location available in destination string. ⊗
MOVB (R1)+,(R0)+;Copy a byte
BNE TACK ;Repeat while necessary
DEC R0 ;Go back past the null
RTS PC ;Done
.MACRO TACKST B ;tack the string B
MOV #B,R1
JSR PC,TACK
.ENDM
.MACRO TACKC B ;tack the character B
MOVB #B,(R0)+ ;move in the value
.ENDM
; following routines are used to get a different form for printing
; R0 will point to next place in the string
PRVAL: PUSH <R2> ;save R2
EVWAIT CSLEVT
MOV #4,R0
MOV #2,R1 ; set format parameters to 2 dec places and squueze out blanks
JSR PC,FORMAT ; use format to squeeze out blanks
FETCH <R1> ; get type of printing
ASH #1,R1 ; TIMES 2
MOV #OUTBUF,R0 ; set R0←start of buffer
JSR PC,@1$-2(R1); call appropriate routines to build up string
CLRB (R0) ; ensure last character is a null to get rid of garbage
MOV #OUTBUF,R0 ; now print it
JSR PC,TYPSTR
JSR PC,RSTFOR ; restore format
EVSIG CSLEVT
POP <R2> ; restore r2
CCC
RTS PC
DATA
1$: PRSCA
PRVEC
PRROT
PRTRN
PRFRM
CODE
PRSCA: MOV (R3)+,R2 ;R2←LOC[value cell]
PRREAL: LDF (R2)+,AC0
JSR PC,CVF ; go the conversion
RTS PC
PRVEC: MOV (R3)+,R2
PVECT: TACKST VNAMEL ; tack "VECTOR("
JSR PC,PRREAL ; tack first value
TACKC COMMA
JSR PC,PRREAL ; second value
TACKC COMMA
JSR PC,PRREAL ; third value
TACKC ') ;")"
RTS PC
PRROT: PUSH <R0>
MOV (R3)+,R0
MOV #EDAT,R1
JSR PC,EULER ; change to EULER angles
MOV #EDAT+14,R2 ; correct address for R2
POP <R0>
PROT: TACKST ROTZHC ; tack ROT(ZHAT,
JSR PC,PRREAL ; value
TACKC ')
TACKC '*
TACKST ROTYHC ; print ROT(YHAT,
JSR PC,PRREAL
TACKC ')
TACKC '*
TACKST ROTZHC ; print ROT(ZHAT,
JSR PC,PRREAL
TACKC ')
RTS PC
PRTRN: MOV #TNAMEL,R1 ; print "TRANS("
JMP PRFRM0
PRFRM: MOV #FNAMEL,R1 ; print "FRAME("
PRFRM0::JSR PC,TACK
JSR PC,PRROT ; use common code with PRROT to compute euler angles
; and tack the rot part
TACKC COMMA ; output a comma
MOV #EDAT,R2
JSR PC,PVECT ; print out the vector part
TACKC ') ; print out right paren
RTS PC
DATA
VNAMEL: .ASCIZ /VECT(/
TNAMEL:: .ASCIZ /TR(/
FNAMEL:: .ASCIZ /FR(/
ROTZHC:: .ASCIZ /ROT(Z,/
ROTYHC:: .ASCIZ /ROT(Y,/
.EVEN
CODE
; supplementary motions: pmove,tadrive,tddrive,gather,rforce,setstf,setspeed
RPMOVE: MOV LRPMOVE,R2 ;set for position independent pcode
JMP MOVST3
RTADRIVE: ; absolute drive
MOV LRTADRIVE,R2
JMP MOVST3
RTDDRIVE: ; relative drive
MOV LRTDDRIVE,R2
JMP MOVST3
RCENTER:
MOV LCENTER,R2 ; used to be LRCENTER
JMP MOVST3
PRETRY: MOV (R3),-(R3) ;copy the address in the stack
JSR PC,GTINT ;R0←addr of move statement
MOV R0,IPC(R4) ;change value of IPC
RTS PC ; and go retry the move
MDONE: TST (R3)+ ;Pop stack
CCC ;Clear condition code.
RTS PC ;Done
PUSHPC: MOV IPC(R4),R0 ; push ipc onto the stack
JMP PUSHI0
COMMENT ⊗ Since addresses of pcode in POINTY are relative, this routine
fills up a temporary block of pcode in a form digestable to
movsta, which it then jumps to
Dont mess up R2 in this routine!
⊗;
MOVST3:
PUSH <R2> ;save R2 for future use
PUSH <IPC(R4)> ; -(SP)←IPC
FETCH <R1> ; R1←old relative address of coef list
ASL R1 ; change to bytes
ADD (SP),R1 ; get absolute address of coef list
PUSH <R1>
FETCH <R1> ; R1←mechanism word
MOV R1,R0
JSR PC,@LMECHNM ; Get bit position
DEC R0
ASL R0 ; Get offset
MOV ABLK(R0),R0 ; Get address of buffer
MOV R0,R2 ; save a copy of R0
POP <(R0)+> ; address of coef list
MOV R1,(R0)+ ; mechanism word passed thro unchanged
FETCH <(R0)+> ; error bits passed through unchanged
FETCH <R1> ; relative address of next pcode with respect to old ipc
ASL R1 ; change to bytes
ADD (SP),R1 ; R1←absolute address of next pcode
MOV R1,(R0)+ ; 4th word after move command
FETCH <R1> ; relative location of retry address
ASL R1 ; change to bytes
ADD (SP),R1 ; get absolute retry address
MOV R1,(R0)+ ; 5th word after move command
MOV #XJUMP,(R0)+ ; jump to the error handling code
MOV IPC(R4),(R0) ; this takes care of jump to error handling code
TST (SP)+ ; pop old value of ipc
MOV R2,IPC(R4) ; change ipc to this temporary block
POP <R2> ; restore R2
JMP MOVSTA ; let AL handle this and return
POPERATE:
PUSH <IPC(R4)> ;save the current ipc address
FETCH <R2> ;save servo bits
FETCH <R1> ;save command bits
FETCH <R0> ; mechanism bits
PUSH <R0> ; save mech bits
JSR PC,@LMECHNM ; Get bit position
DEC R0
ASL R0 ; Get offset
MOV ABLK(R0),R0 ; Get address of buffer
PUSH <R0> ; R0←address
MOV R2,(R0)+ ; servo bits passed thro unchanged
MOV R1,(R0)+ ; command bits passed thro unchanged
POP <R2> ; address of the pcode
POP (R0)+ ; send mech word across
FETCH <(R0)+> ; error bits passed through unchanged
FETCH <R1> ; relative address of next pcode with respect to old ipc
ASL R1 ; change to bytes
ADD (SP),R1 ; R1←absolute address of next pcode
MOV R1,(R0)+ ; write into list to be passed to operate
FETCH <R1> ; relative location of retry address
ASL R1 ; change to bytes
ADD (SP),R1 ; get absolute retry address
MOV R1,(R0)+ ; 6th word after move command
MOV #XJUMP,(R0)+ ; jump to the error handling code
MOV IPC(R4),(R0) ; this takes care of jump to error handling code
TST (SP)+ ; pop old value of ipc
MOV R2,IPC(R4) ; set ipc to temporary block
JMP OPERATE ; let AL handle this and return
DATA
YRBLK: .BLKW 7 ; block for yellow arm
YHBLK: .BLKW 7 ; block for yellow hand
BRBLK: .BLKW 7 ; block to set up stuff for blue arm
BHBLK: .BLKW 7 ; block for blue hand
VBLK: .BLKW 7 ; block for vise
SBLK: .BLKW 7 ; block for screwdriver
ABLK: .WORD YRBLK,YHBLK,BRBLK,BHBLK,VBLK,SBLK
CODE
DATA
;SVPTR: 0 ;used in case we do a RETRY$G
;RPFLAG: 0 ;checks if we did a RETRY$G
CODE
GATHER: FETCH <R0>
MOV #FPPTR,R1 ;address of FP buffer
MOV #INTPTR,R2 ;address of INTEGER buffer
PUSH <R3> ;save it for now
MOV #XMOVE,R3 ;pass control word to arm code
JSR PC,@LGATHER ; now go call the appropriate routine
POP <R3> ;restore R3
RTS PC
RFORCE: SNDINT #XRFORCE ;send back a xrforce
MOV #INTPTR,R1 ;address of integer buffer
JSR PC,@LRFORCE
CCC
RTS PC
SETSTF: MOV (R3)+,-(SP) ; save trans address
MOV #1$+24.,R0 ; address of arguments
MOV #6,R1 ; six of them
2$: LDF @(R3)+,AC0 ; get the argument
STF AC0,-(R0) ; put in the right place
SOB R1,2$
; MOV #1$,R0 ; let R0 point to the right place
; R0 will be pointing to the right place
MOV (SP)+,R1 ; R1 has address of trans
JSR PC,@LSETSTF ; jump into the arm code
CCC
RTS PC ; and return
DATA
1$: .BLKW 12. ; space for 6 real numbers
CODE
STIF0: MOV #2$,R0 ; R0←LOC[six scalars]
MOV #TR0,R1 ; niltrans
JSR PC,@LSETSTF ; jump into the arm code
CCC
RTS PC
DATA
2$: .FLT2 90.0,90.0,90.0,20000.0,20000.0,20000.0
CODE
PWRIST: MOV #6*2,R0 ;Get enough room to store 6 floating point force values
JSR PC,GTFREE
MOV R0,R1 ;R1 ← address of device block
PUSH <R0> ;Save a copy on the stack
CLR R0 ;Use internal calibration matrix
JSR PC,@LWRIST ;Go read the wrist
JSR PC,GTARGS ;R0 ← LOC[env entry for force vector:K]
PUSH <R0> ;Save it
JSR PC,GTARGS ;R0 ← LOC[env entry for torque vector:G]
PUSH <R0> ;Save this one too
JSR PC,GETVEC ;R0 ← -(R3) ← LOC[new vector]
JSR PC,GETVEC ;R0 ← -(R3) ← LOC[new vector] - Get two of them
POP <R0,R1> ;R0 ← G, R1 ← K
MOV (R3),2(R1) ;Store pointer to force vector away in environment
MOV 2(R3),2(R0) ; ditto for torque vector
MOV (SP),R2 ;R2 ← LOC[force components]
MOV #2,R0 ;# of vectors to transfer
1$: MOV (R3)+,R1 ;R1 ← LOC[force/torque vector]
LDF (R2)+,AC0 ;Get 1st force component
STF AC0,(R1)+ ;Store it in vector
LDF (R2)+,AC0 ; ditto for 2nd component
STF AC0,(R1)+
LDF (R2)+,AC0 ; & likewise for 3rd component
STF AC0,(R1)+
SOB R0,1$ ;Do both vectors
POP <R0> ;R0 ← LOC[force component block]
JSR PC,RLFREE ;Release it
CCC
RTS PC ;All done
SETSPEED:
LDF @(R3)+,AC0 ;AC0←speed_factor
CMPF ONE,AC0 ;compare that it is greater than 1
CFCC ;copy condition codes
BLE 1$ ; OK
LDF TWO,AC0 ; Default speed = 2.0
ALERR 3$ ; complain too fast
1$: JSR PC,@LSETSPEED
RTS PC ; done
DATA
3$: .ASCIZ /
SPEED FACTOR MUST BE GREATER THAN 1. <alt>P WILL SET IT TO 2.0/
CODE
; supplementary functions: uparrow,dwnarrow,alpha,dollar,swap,vneg,vsmul,ftof
UPARROW: MOV #PZHAT,-(R3) ; ↑ z-axis pointing upward, current frame or trans
MOV 2(R3),R0 ; get original trans value
LDF (R0),AC0
MULF AC0,AC0 ; (1,1)↑2
LDF 4(R0),AC1
MULF AC1,AC1 ; (2,1)↑2
ADDF AC1,AC0 ; ACO←(1,1)↑2+(2,1)↑2
CMPF C0001,AC0 ; If AC0<C001 skip ahead
CFCC
BGT 1$
CLRF AC0
SUBF 10(R0),AC0 ; -(3,1)
JSR PC,@LASIN ; take arc-sin
BR 2$
1$: LDF 34(R0),AC0
LDF 30(R0),AC1
JSR PC,@LATAN2 ; take arc-tan2( (2,3),(1,3))
2$: JSR PC,GETSCA ;R0 ← -(R3) ← LOC[new scalar block]
STF AC0,@(R3) ;Store result
BR DW3 ;produce the rot
DOLLAR: MOV #NILROT,-(R3) ; $ station orientation, i.e. nilrot
BR DW2
ALPHA: MOV #PZHAT,-(R3) ; bgrasp orien at bpark, e.e. rot(zhat,180)
BR DW1
DWNARROW: MOV #PYHAT,-(R3) ; ↓ bpark orien, i.e. rot(yhat,180)
DW1: MOV #F180,-(R3) ; rot of 180 deg
DW3: JSR PC,VSAXWR ; return rot(vect,180) on stack
DW2: JSR PC,SWAP ; turn the top two elements around
JSR PC,TPOS ; take the position value of previous frame
JSR PC,TMAKE ; produce the transform
RTS PC ; and return
VNEG: MOV (R3),-(R3) ; copy the vector on the stack
MOV #NILVEC,2(R3) ; put in nilvector
JMP VSUB
VSMUL: JSR PC,SWAP ; reverse the two top elements
JMP SVMUL ; exit from SVMUL
SWAP: MOV (R3),-(SP) ; switch positions of top two elementsof stack
MOV 2(R3),(R3)
MOV (SP)+,2(R3)
RTS PC
WRT: JSR PC,TORIEN ; v wrt t = orient(t)*v
VFREL: JSR PC,SWAP ; v rel f = t*v
JMP TVMUL
FTOF: JSR PC,SWAP ;t1→t2 = inv(t1)*t2
JSR PC,TINVRT
FFREL: JSR PC,SWAP ; f rel t = t*f
JMP TTMUL
; take positions of three frames and put them
; to the stack
FCONSTR: MOV (R3)+,-(SP) ; save top two elements
MOV (R3)+,-(SP)
JSR PC,TPOS ; find position of frame 1
MOV (SP)+,-(R3)
JSR PC,TPOS ; find position of frame 2
MOV (SP)+,-(R3)
JSR PC,TPOS ; find position of frame 3
JMP CONSTR
TVREL: MOV #TR0,-(R3) ; (R3)←niltrans
JSR PC,SWAP ; swap it around
JSR PC,TMAKE ; make it into trans(nilrot,v)
JMP TTMUL ; return from TTMUL
; functions: sqrt,sin,cos,asin,acos,tan,atan2,log,exp
COMMENT ⊗
PSQRT: LDF @(R3)+,AC0 ;AC0 ← arg
JSR PC,SQRT
JMP SRET
PSIN: LDF @(R3)+,AC0 ;AC0 ← arg
JSR PC,SIN
JMP SRET
PCOS: LDF @(R3)+,AC0 ;AC0 ← arg
JSR PC,COS
JMP SRET
PTAN: LDF @(R3)+,AC0 ;AC0 ← arg
JSR PC,TAN
JMP SRET
PASIN: LDF @(R3)+,AC0 ;AC0 ← arg
JSR PC,ASIN
JMP SRET
PACOS: LDF @(R3)+,AC0 ;AC0 ← arg
JSR PC,ACOS
JMP SRET
⊗ ;
PATAN2: JSR PC,SWAP
LDF @(R3)+,AC0 ;AC0 ← arg
JSR PC,ATAN2
JMP SRET
COMMENT ⊗
PLOG: LDF @(R3)+,AC0 ;AC0 ← arg
JSR PC,LOG
JMP SRET
PEXP: LDF @(R3)+,AC0 ;AC0 ← arg
JSR PC,EXP
JMP SRET
⊗;
; armreach- can arm reach here?
; routine checks if arm can reach location specified on the stack
; it leaves true or false on the stack
COMMENT ⊗
ARMREACH:
PUSH <R2> ; save R2
MOV #28.,R0 ; angle list
JSR PC,GTFREE
PUSH <R0>
MOV #14.,R0
JSR PC,GTFREE ; pointer list
PUSH <R0>
MOV 2(SP),R1 ;R1←address of angle values
MOV #14.,R2 ; shift 14 addresses
1$: MOV R1,(R0)+
ADD #4,R1
SOB R2,1$
MOV (R3)+,R0 ;R0←LOC[trans]
MOV (SP),R1 ;R1←address pointers
FETCH <R2> ;R2←mechanism
;;; JSR PC,LSOLVE ; jump into armsolution routine
PUSH <R0> ; save error code
JSR PC,GETSCA ; R0←-(R3)←LOC[scalar]
MOV ONE,(R0)+ ; put scalar as true
CLR (R0)
TST (SP)+ ; check error code from SOLVE
BEQ 2$ ; there was no error
CLR (R3) ; oops there was an error
2$: POP <R0>
JSR PC,RLFREE ; release theta pointer space
POP <R0>
JSR PC,RLFREE ; release space for theta angles
POP <R2> ; restore R2
CCC
RTS PC ; return
⊗;
; procedure handling: GTBLK
GTBLK:
COMMENT ⊗
GTBLK n ..... q
n is size of the block of pcode to be copied
..... is n words of information
the address of the block is to be put at the location of q + offset q
⊗
FETCH <R0> ; get size of the block to get
MOV R0,R2 ;
; ADD R0,R0 ; get size in bytes
JSR PC,GTFREE ; get the size we need
MOV R0,-(SP) ; save the address of the block
1$: FETCH <R1> ; get word to transfer
MOV R1,(R0)+ ; transfer to new area
SOB R2,1$
MOV @IPC(R4),R1 ; now get the offset in which to stick the address of this block
ASL R1 ; get it in bytes
ADD IPC(R4),R1 ; get the absolute address
BMPIPC
MOV (SP)+,(R1) ; write into the pcode ####### ... careful !
RTS PC ; and return
; more stack ops: gtint,gvals,chngs
APUSHOFFSET:
JSR PC,PUSHINITI ; push index onto stack
; The argument is an integer. Make a scalar record and store the offset value
; on that stack.
; this routine is used in conjunction with GVALS and CHNGS
JMP PUSHINTI
GTINT: LDF @(R3)+,AC0 ;Get value of top element of stack
STCFI AC0,R0 ;Convert it to integer & store it in R0
RTS PC
GVALS: JSR PC,GTINT ; get the value of variable whose offset is on stack
JMP GVAL0
CHNGS: JSR PC,GTINT ; change the value of the variable whose offset is on stack
JMP CHNG0
GTARGS: JSR PC,GTINT ; take the value from the stack and convert to integer
JMP GETARG
; components of data types: CHCMP,GTCMP
; appropriate component of element whose level offset is on stack is changed
; or obtained
CHCMP: FETCH <R0>
DEC R0 ;reduce by 1
ASH #2,R0 ;multiply by 4
MOV R0,-(SP)
JSR PC,GTARGS ; R0←[env entry]
MOV R0,-(SP) ; save for later use
JSR PC,GVAL1 ; (R3)←LOC[vect or trans]
MOV (R3),R0
CMPB #VCTID,TAGID(R0); check if it is a vector
BEQ 1$ ; yes it is
ADD #44,2(SP) ; no, it isnt
1$: JSR PC,SWAP ; trade two top elements of stack so scalar on top
LDF @(R3)+,AC0 ; AC0← value of component to be changed
MOV 2(SP),R0 ; put component into R0
ADD (R3),R0 ; get effective address of component
STF AC0,(R0) ; (R3) has appropriate value
MOV (SP)+,R0 ; get back environment entry
JSR PC,CHNG1 ; and change the value
TST (SP)+ ; pop the stack
RTS PC
CHTPOS: JSR PC,GVALS
MOV #44,R0 ; put the offset into R0
ADD (R3)+,R0 ; R0←LOC[x-comp of trans]
MOV (R3)+,R1 ; R1←LOC[x-comp of vector]
PUSH <R2>
MOV #3,R2 ; use R2 as counter
1$: LDF (R1)+,AC0
STF AC0,(R0)+
SOB R2,1$
POP <R2>
RTS PC
CHTORIENT:
JSR PC,GVALS
MOV (R3)+,R0 ;R0←[LOC trans]
MOV (R3)+,R1
PUSH <R2> ;use R2 as counter
MOV #9.,R2 ;transfer 9 elements
1$: LDF (R1)+,AC0
STF AC0,(R0)+
SOB R2,1$
POP <R2>
RTS PC
COMMENT ⊗
GTXC: CLR R1
BR GTCMP0
GTYC: MOV #4,R1
BR GTCMP0
GTZC: MOV #10,R1
BR GTCMP0
⊗ ;
GTCMP: FETCH <R1>
DEC R1
ASH #2,R1
GTCMP0::MOV (R3),R0
ADD (R3)+,R1 ; save on the stack
CMPB #VCTID,TAGID(R0); is it a vector?
BEQ 1$ ; yes, it is
ADD #44,R1 ; no, it is a trans
1$: LDF (R1),AC0 ;AC0←Appropriate value
JMP PUSHREAL ; push into stack and return from there
; signal,wait,cmpwait,cmvar,cmfil,pkvar
PSIGNAL:JSR PC,GTINT ;R0 ← level-offset pair.
JMP SIGNL0 ; return from AL
PWAIT: JSR PC,GTINT ;R0 ← level-offset pair.
JMP WAITE0 ; return from AL
PCMWAIT:JSR PC,GTINT ;R0 ← level-offet pair
JMP CMWAI0 ;return from AL
CMVAR:
; sets up the cmon, but does not create the cmon or its control block
; or fill in the body the way MVAR does.
MOV ENV(R4),R2 ;R2 ← LOC[current environment]
MOV LVARS(R2),R2 ;R2 ← LOC[first free entry in environment]
FETCH R0 ;Get count of # of cmons declared
1$: MOV #CMNTYP,(R2)+ ;just stick data type in place
CLR (R2)+ ; & zero the value pointer
SOB R0,1$ ; for each one
MOV ENV(R4),R0 ;R0 ← LOC[environment]
MOV R2,LVARS(R0) ;Update first free variable entry
CCC
RTS PC
CMFIL:
; fills in the body of the cmon which has been declared previously by cmvar
FETCH <R0> ;R0←levoff
JSR PC,GETENV ;R0←environment entry
MOV R0,R2 ;R2←env entry
MOV #1,R0 ;to set it up right for CMMAK
JMP CMMAK ;go make the cmon and return directly
PKVAR:
; if argument > 0 then calls KVAR otherwise if no-op
FETCH <R1> ; R1←#of variables to kill
TST R1
BGT 1$
RTS PC
1$: JMP KVAR0 ; return from KVAR
; return from POINTY : pdone
PDONE:
MOV RF,SP ;Restore stack
MOV -2(SP),RF ;RF ← old PC
RTS RF ;Just return